home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Users Group Library 1996 July / C-C++ Users Group Library July 1996.iso / listings / v_09_11 / 9n11052a < prev    next >
Text File  |  1991-09-23  |  12KB  |  354 lines
  1. /* -- simpwalk.h --
  2.  */
  3. #define LEGDOWN_CYCLE_TIME                      50
  4. #define ALPHA_ADVANCE_CYCLE_TIME        10
  5. #define GAIT_CYCLE_TIME                         500
  6.  
  7. typedef enum { ALPHA_DECR = -1, NEUTRAL, ALPHA_INCR } POSITION;
  8. #define BETA_UP         127
  9. #define BETA_DOWN       -128
  10.  
  11. #define IS_BETA_UP(x)   !IS_BETA_DOWN(x)
  12. #define IS_BETA_DOWN(x) ((x) < 0)
  13.  
  14. typedef struct
  15.         {
  16.         char    *inName;
  17.         char    *outName;
  18.         int             servoName;
  19.         } IO_NAME;
  20.  
  21. extern IO_NAME alphaData[], betaData[];
  22. void alphaPos(void), betaPos(void);
  23.  
  24. /* -- standup.c --
  25.  */
  26. #include <stdio.h>
  27. #include "exec.h"
  28. #include "simpwalk.h"
  29.  
  30. #define alpha0Servo 0
  31. #define alpha1Servo 1
  32. #define alpha2Servo 2
  33. #define alpha3Servo 3
  34. #define alpha4Servo 4
  35. #define alpha5Servo 5
  36. #define beta0Servo  6
  37. #define beta1Servo  7
  38. #define beta2Servo  8
  39. #define beta3Servo  9
  40. #define beta4Servo  10
  41. #define beta5Servo  11
  42.  
  43. IO_NAME alphaData[] = {
  44.         { "alpha0Pos.in", "alpha0Pos.out", alpha0Servo },
  45.         { "alpha1Pos.in", "alpha1Pos.out", alpha1Servo },
  46.         { "alpha2Pos.in", "alpha2Pos.out", alpha2Servo },
  47.         { "alpha3Pos.in", "alpha3Pos.out", alpha3Servo },
  48.         { "alpha4Pos.in", "alpha4Pos.out", alpha4Servo },
  49.         { "alpha5Pos.in", "alpha5Pos.out", alpha5Servo }
  50.         };
  51.  
  52. IO_NAME betaData[] = {
  53.         { "beta0Pos.in", "beta0Pos.out", beta0Servo },
  54.         { "beta1Pos.in", "beta1Pos.out", beta1Servo },
  55.         { "beta2Pos.in", "beta2Pos.out", beta2Servo },
  56.         { "beta3Pos.in", "beta3Pos.out", beta3Servo },
  57.         { "beta4Pos.in", "beta4Pos.out", beta4Servo },
  58.         { "beta5Pos.in", "beta5Pos.out", beta5Servo }
  59.         };
  60.  
  61. /*      alpha position.  One per leg.
  62.  */
  63. void alphaPos(void)
  64.         {
  65.         IO_NAME *pData = &alphaData[ExecArg0];
  66.         PORT portValue, position;
  67.         STRING_ATOM output = StringToAtom(pData->outName);
  68.  
  69.         MsgOpenPort(output, &portValue);
  70.         MsgWriteToPort(output, 
  71.                         servoWrite(pData->servoName, NEUTRAL));
  72.         MsgWatchPort(StringToAtom(pData->inName), &position);
  73.         while (MsgGetMsg())
  74.                 {
  75.                 MsgWriteToPort(output, 
  76.                                 servoWrite(pData->servoName, position));
  77.                 }
  78.         }
  79.  
  80. /*      beta position.  One per leg.
  81.  */
  82. void betaPos(void)
  83.         {
  84.         IO_NAME *pData = &betaData[ExecArg0];
  85.         EXEC_ID id, timerID;
  86.         PORT currentPos, portValue, position;
  87.         STRING_ATOM output = StringToAtom(pData->outName);
  88.  
  89.         MsgOpenPort(output, &portValue);
  90.         MsgWriteToPort(output, BETA_DOWN);
  91.  
  92.         servoWrite(pData->servoName, position = BETA_DOWN);
  93.         while (servoReadWrite(pData->servoName, position) != BETA_DOWN)
  94.                 ;
  95.         MsgWatchPort(StringToAtom(pData->inName), &position);
  96.         timerID = MsgWatchTimer(1);
  97.  
  98.         while (id = MsgGetMsg())
  99.                 {
  100.                 if (id == timerID)
  101.                         MsgWriteToPort(output,
  102. servoReadWrite(pData->servoName, position));
  103.                 else
  104.                         servoWrite(pData->servoName, position);
  105.                 }
  106.         }
  107.  
  108. /* -- simpwalk.c --
  109.  */
  110. #include <stdlib.h>
  111. #include "exec.h"
  112. #include "simpwalk.h"
  113.  
  114. /*      interface names
  115.  */
  116. IO_NAME legDownData[] = {
  117.         { "leg0Down.in", "leg0Down.out" },
  118.         { "leg1Down.in", "leg1Down.out" },
  119.         { "leg2Down.in", "leg2Down.out" },
  120.         { "leg3Down.in", "leg3Down.out" },
  121.         { "leg4Down.in", "leg4Down.out" },
  122.         { "leg5Down.in", "leg5Down.out" }
  123.         };
  124.  
  125. IO_NAME advanceData[] = {
  126.         { "alpha0Advance.in", "alpha0Advance.out" },
  127.         { "alpha1Advance.in", "alpha1Advance.out" },
  128.         { "alpha2Advance.in", "alpha2Advance.out" },
  129.         { "alpha3Advance.in", "alpha3Advance.out" },
  130.         { "alpha4Advance.in", "alpha4Advance.out" },
  131.         { "alpha5Advance.in", "alpha5Advance.out" }
  132.         };
  133.  
  134. IO_NAME uplegData[] = {
  135.         { "upleg0Trigger.in", "upleg0Trigger.out" },
  136.         { "upleg1Trigger.in", "upleg1Trigger.out" },
  137.         { "upleg2Trigger.in", "upleg2Trigger.out" },
  138.         { "upleg3Trigger.in", "upleg3Trigger.out" },
  139.         { "upleg4Trigger.in", "upleg4Trigger.out" },
  140.         { "upleg5Trigger.in", "upleg5Trigger.out" }
  141.         };
  142.  
  143. /*      put the leg down if it's up.  One per leg.
  144.  */
  145. void legDown(void)
  146.         {
  147.         IO_NAME *pData = &legDownData[ExecArg0];
  148.         PORT legPosition;
  149.         STRING_ATOM betaOut, output;
  150.  
  151.         betaOut = StringToAtom(betaData[ExecArg0].outName);
  152.         output = StringToAtom(pData->outName);
  153.         /*      connect our output to the beta position's input
  154.          */
  155.         MsgConnectPorts(output, StringToAtom(betaData[ExecArg0].inName));
  156.         MsgWatchTimer(LEGDOWN_CYCLE_TIME);
  157.  
  158.         while (MsgGetMsg())
  159.                 {
  160.                 MsgPeekPort(betaOut, &legPosition);
  161.                 if (IS_BETA_UP(legPosition))
  162.                         MsgWriteToPort(output, BETA_DOWN);
  163.                 }
  164.         }
  165.  
  166. /*      if the total alpha positions are positive, then move them back.
  167.  *      If it is negative, move them forward.  One for all legs.
  168.  */
  169. void alphaBalance(void)
  170.         {
  171.         STRING_ATOM alpha0, alpha1, alpha2, alpha3, alpha4, alpha5;
  172.         STRING_ATOM alphaOut;
  173.         PORT totalAlphaPosition;
  174.  
  175.         alpha0 = StringToAtom(alphaData[0].outName);
  176.         alpha1 = StringToAtom(alphaData[1].outName);
  177.         alpha2 = StringToAtom(alphaData[2].outName);
  178.         alpha3 = StringToAtom(alphaData[3].outName);
  179.         alpha4 = StringToAtom(alphaData[4].outName);
  180.         alpha5 = StringToAtom(alphaData[5].outName);
  181.         alphaOut = StringToAtom("alphaBalance.out");
  182.         /*      connect our output to the alpha motors' input
  183.          */
  184.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[0].inName));
  185.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[1].inName));
  186.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[2].inName));
  187.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[3].inName));
  188.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[4].inName));
  189.         MsgConnectPorts(alphaOut, StringToAtom(alphaData[5].inName));
  190.  
  191.         MsgWatchTimer(ALPHA_ADVANCE_CYCLE_TIME);
  192.         while (MsgGetMsg())
  193.                 {
  194.                 totalAlphaPosition =
  195.                         MsgPeekPort(alpha0, NULL) +
  196.                         MsgPeekPort(alpha1, NULL) +
  197.                         MsgPeekPort(alpha2, NULL) +
  198.                         MsgPeekPort(alpha3, NULL) +
  199.                         MsgPeekPort(alpha4, NULL) +
  200.                         MsgPeekPort(alpha5, NULL);
  201.                 if (totalAlphaPosition < 0)
  202.                         MsgWriteToPort(alphaOut, ALPHA_INCR);
  203.                 else if (0 < totalAlphaPosition)
  204.                         MsgWriteToPort(alphaOut, ALPHA_DECR);
  205.                 }
  206.         }
  207.  
  208. /*      if the leg is up, then move leg forward.  One per leg.
  209.  */
  210. void alphaAdvance(void)
  211.         {
  212.         PORT betaPosition;
  213.         STRING_ATOM input, output, alphaPosInput;
  214.  
  215.         /*      wakes up whenever the beta position changes
  216.          */
  217.         input = StringToAtom(advanceData[ExecArg0].inName);
  218.         MsgWatchPort(input, &betaPosition);
  219.  
  220.         alphaPosInput = StringToAtom(alphaData[ExecArg0].inName);
  221.         output = StringToAtom(advanceData[ExecArg0].outName);
  222.  
  223.         /*      change alpha position input to connect to our output
  224.          */
  225.         MsgInhibitPorts(output, alphaPosInput);
  226.         MsgConnectPorts(StringToAtom(betaData[ExecArg0].outName), input);
  227.  
  228.         while (MsgGetMsg())
  229.                 {
  230.                 if (IS_BETA_UP(betaPosition))
  231.                         MsgWriteToPort(output, ALPHA_INCR);
  232.                 }
  233.         }
  234.  
  235. /*      if the leg is down, move it up
  236.  */
  237. void uplegTrigger(void)
  238.         {
  239.         PORT betaPosition;
  240.         STRING_ATOM betaInput, betaOutput, output;
  241.  
  242.         betaInput = StringToAtom(bet